"""智能小车"""
import pyb
from pyb import *
import time
import pyb
from pyb import Pin
from pyb import Timer

# 避障, 循迹, 超声波避障
class my_agv():
    def __init__(self):
        # 红外避障对象
        self.M1 = Pin('X5', Pin.IN)
        self.M2 = Pin('X6', Pin.IN)
        self.M3 = Pin('X7', Pin.IN)
        self.M4 = Pin('X8', Pin.IN)
        # 点击控制 PIN  N1, N2左轮, N3, N4 右轮
        self.N1 = Pin('Y1', Pin.OUT_PP)
        self.N2 = Pin('Y2', Pin.OUT_PP)
        self.N3 = Pin('Y3', Pin.OUT_PP)
        self.N4 = Pin('Y4', Pin.OUT_PP)
        # 运行指示灯
        self.LED4 = pyb.LED(4)
        self.LED3 = pyb.LED(3)
        self.LED2 = pyb.LED(2)

        # 速度
        self.speed1 = 0.007
        self.speed2 = 0.01

    def __del__(self):
        self.LED4.off()
        self.LED3.off()
        self.LED2.off()
        self.AGV_stop()  # 电机停止工作

    # 电机停止工作
    def AGV_stop(self):
        self.N1.low()
        self.N2.low()
        self.N3.low()
        self.N4.low()

    # 直行  self.speed1, self.speed2 为限速值
    def straight(self):
        self.LED3.on()
        count = 0
        try:
            self.N1.low()
            self.N2.high()
            self.N3.low()
            self.N4.high()
            time.sleep(self.speed1)  # 这里时间差越大, 速度越慢
            self.AGV_stop()
            time.sleep(self.speed2)
            count += 1
        except Exception as e:
            print(e)

    # 智能车左转
    def M4_get_M2_M3(self):
        while True:
            time.sleep(0.01)
            self.N1.low()
            self.N2.high()
            self.N3.high()
            self.N4.low()
            if self.M2.value() == 1 or self.M3.value() == 1:
                break
            elif self.M1.value() == 1:
                self.M1_get_M2_M3()
            else:
                time.sleep(0.01)
                self.AGV_stop()  # 电机暂停工作

    # 智能车右转
    def M1_get_M2_M3(self):
        while True:
            time.sleep(0.01)
            self.N1.high()
            self.N2.low()
            self.N3.low()
            self.N4.high()
            if self.M2.value() == 1 or self.M3.value() == 1:
                time.sleep(0.01)
                break
            elif self.M4.value() == 1:
                self.M1_get_M2_M3()
            else:
                time.sleep(0.01)
                self.AGV_stop()

    # 智能车直行
    def go_go(self):
        while True:
            self.straight()
            if (self.M1.value() == 1):
                self.M1_get_M2_M3()
                break
            if (self.M4.value() == 1):
                self.M4_get_M2_M3()
                break

    def agv_test(self):
        self.LED3.on()

        self.N1.low()
        self.N2.high()
        self.N3.low()
        self.N4.high()
        time.sleep(10)
        self.AGV_stop()

        self.LED3.off()

    # 智能车
    def start_run(self):
        print("agv2限速运行版开始运行0.01")
        try:
            self.AGV_stop()  # 电机停止工作
            while True:
                if (self.M1.value() == 1):  # 检测到物体返回0。
                    self.M1_get_M2_M3()

                if (self.M4.value() == 1):  # 检测到物体返回0。
                    self.M4_get_M2_M3()

                if self.M3.value() == 1 or self.M2.value() == 1:
                    self.go_go()    # 执行

        except:
            self.AGV_stop()
            print("异常退出!!!!")



